%Given an planar orbit in the 10^-3 neighborhood of L2, the prigram follows
%it along its cylinder to out of plane halo orbits

%clear the workspace
clear;
%output long
format long;


%--------------------------------------------------------------------------
%----------------------NOTES-----------------------------------------------
%--------------------------------------------------------------------------

%NAME:  haloFamilyL2.m

%DEPENDANCIES: The program calls 'CRTBP.m', 'librationPoints.m', 
%'stateTransCRTBP.m'and 'jacobiConst.m'.  The program 'stateTransCRTBP
%calls 'sysSolveCRTBP', which calls 'G_CRTBP.m'.

%These must be in the MatLab search path in order for the present
%program to run.

%USE: Once the program and it's dependencies are in the search path simply
%type:
%                  haloFamilyL2
%
%from the matlab command prompt.

%OUTPUT: The output is a plot of the Halo family


%PURPOSE: Begining from a lyapunov orbit near bifurcation, the program 
%computes the continuation of the orbit out of plane and follows it for a 
%specified number of orbits at a specified step size.  This produces the 
%cylinder of halo orbits parametrized by energy.

%--------------------------------------------------------------------------
%----------------------USER SPECIFIED--------------------------------------
%------------------------PARAMETERS----------------------------------------
%--------------------------------------------------------------------------


%Number of orbits in family to find
M=3;
%interval=abs((1-mu)-(r0(1) - 0.00001));
%familyStep=interval/M;

%Step:
familyStep=0.0015;



%----------------------DATA------------------------------------------------

%parameters

%Gravational constant
G=1;

%We are given data for the Sun/Earth/Moon system where the sun is the
%primary, and the earth and moon are combined into one body at their center
%of mass.  The earth moon system is refered to as the earth.  Then

GM_sun=1.327*10^(11);
GM_earth=3.986004418*10^(5);

%The definition of mu gives
mu=GM_earth/(GM_sun+GM_earth);

m1=1-mu;
m2=mu;

au=1.496*10^8;

%initial position guess for planar Lyapunov orbit near bifurcation.  Small
%delta z is added to make it a guess for an out of plane orbit.
r0=[1.00842815565444;
                  0;
             0.0001];
%The velociety of the lyapunov orbit.
v0=[              0;
    0.0098103930652;
                  0];

%v0=(1/au)*(60*60*24*365.25)/(2*pi)*[0; -0.3275035; 0]

x0=[r0; v0];

%initial time guess
t_initial=3.1026/2;
%t_initial=1.537


%Compute the Jacobi constant for the given initial conditions
C=jacobiConst(r0,v0,mu);

%---------Libration Points-------------------------------------------------



%Once 'G' and 'mu' are defined we can compute the location of the libration
%points and their Jacobi Integrals.  These will be handy when we set the
%value of the Jacobi Integral for the simulation below.

    %AXULARY CONSTANTS:
    %Compute the location of the five libration points as points in three
    %space.  These can be used to set the value of the Jacobi Constant.
    [L1, L2, L3, L4, L5]=librationPoints(mu);

    %Output location to the screen (if you want);
    outL1=L1;
    outL2=L2;
    outL3=L3;
    outL4=L4;
    outL5=L5;
    %Now compute the Jacobi Energy at each collinear libration point

        %The velocity at any equilibrium is zero.
        v_equil=[0; 0; 0];

    %Compute the energy at each libration point
    CL3=jacobiConst(L3, v_equil, mu);
    CL1=jacobiConst(L1, v_equil, mu);
    CL2=jacobiConst(L2, v_equil, mu);
    CL4=jacobiConst(L4, v_equil, mu);
    CL5=CL4; %so no extra computation is needed


%Aux Matrices
O=zeros(3);
I=eye(3);
K=[0, 2, 0;
  -2, 0, 0;
   0, 0, 0];


%--------------------------------------------------------------------------
%----------------------Newton Method; find orbits--------------------------
%--------------------------------------------------------------------------

%initial guess
x_n=x0;
tau_n=t_initial;
tau=tau_n;


figure
hold on

for j=1:M
    out_j=j;
    K=14;
    %plot3(r0(1),x_n(2),x_n(3),'r*')
    for i=1:K
        out_i=i;
        %We need some infromation from the 'reference trajectory'.  
            t0=0;
            tf=tau_n;
            numSteps=2000;
            tspan=linspace(0, tf, numSteps);
            %numerical integration
            y0=x_n';                                      %inital condition
            options=odeset('RelTol',2.5e-14,'AbsTol',1e-22);%set tolerences
            [~,x_ref]=ode113('CRTBP',tspan,y0,options,[],G,mu);
        %Start computing the differential of the Newton condition vector
        %Need the STM for nth guess at nth time (tau_n is n+1 th time)    
            Phi=stateTransCRTBP(t0, tf, x_n, mu);
            %some of the terms depend on the vector field
            f_x=vectorField_CRTBP(x_ref(numSteps,:),G,mu);
            %Construct the needed differential
            a11=Phi(4,1);
            a12=Phi(4,5);
            a21=Phi(6,1);
            a22=Phi(6,5);
            a31=Phi(2,1);
            a32=Phi(2,5);
            %the differential of the constraint vector
            DF=[a11, a12, f_x(4);
                a21, a22, f_x(6);
                a31, a32, f_x(2)];
            %Invert
            D=inv(DF);
        %Compute next (or final/target) initial conditions    
            %compute next step
        x_star=[x_n(1); x_n(5); tau_n] ...
          -D*[x_ref(numSteps, 4); x_ref(numSteps, 6); x_ref(numSteps, 2)];
     delXo = -D*[x_ref(numSteps, 4); x_ref(numSteps, 6); x_ref(numSteps, 2)]
        %set up x_n for next itteration or output
     %Put it all into x_n (which is really x_(n+1) as it occurs at the end
        %if the loop
            x_n=[x_star(1);
                0;
                r0(3);
                0;
                x_star(2);
                0];
            tau_n=x_star(3);
    end    
    outX=x_n;
    outTime=tau_n;
    CTargetOrbit(j)=jacobiConst(x_n(1:3), x_n(4:6), mu);
    deltaInitialEnergy=abs(C-CTargetOrbit);
    %Now compute the whole orbit using symmetry
    t0=0;
    haloPeriod(j)=2*tau_n;
    numSteps=2000;
    tspan=linspace(0, 5*tau_n, numSteps);
    %numerical integration
    y0=x_n';                                             %inital condition
    options=odeset('RelTol',1e-14,'AbsTol',1e-22);     %set tolerences
    [t,x_halo]=ode113('CRTBP',tspan,y0,options,[],G,mu);
    %Some analysis of the orbit;
    %check the order of magnitude to which the orbit is periodic
    periodicityCheck(j)=norm(x_halo(1,1:3)-x_halo(numSteps,1:3));
    %Compute the monodromy matrix for the periodic orbit
    Phi=stateTransCRTBP(t0, 2*tau_n, x_n, mu);
    %determinant of Phi;
    testDetPhi=det(Phi);
    %Get eigenvalues and eigenvectors
    [Phi_eigVecs, Phi_eigValOnDiag]=eigs(Phi);
    PhiEigenValues(j,1:6)=diag(Phi_eigValOnDiag)';
    %GET THE IC's OF THE HALO FAMILY
    InitialConditions(j,1:6)=x_n';
    %plot the halo trajectory
    plot3(x_halo(:,1), x_halo(:,2), x_halo(:,3), 'b')
    %set the initial condition for the next orbit;
    x_n=x_n-[0;0;familyStep;0;0;0];
    r0(3)=x_n(3);
end


%Simulate the original conditions
%tf=2*tf;
%numSteps=2000;
%tspan=linspace(0,tf,numSteps);
%numerical integration
%y0=x0';                                             %inital condition
%options=odeset('RelTol',1e-14,'AbsTol',1e-22);     %set tolerences
%[t,x]=ode113('CRTBP',tspan,y0,options,[],G,mu);


%plot the libration points
plot3(L3, 0, 0, 'ko', L1, 0, 0, 'ko', ...
    L2, 0, 0, 'ko', L4(1,1), L4(2,1), 0,'ko',  L5(1,1), L5(2,1), 0,'ko')

%plot the planets (primaries)
plot3(-mu,0, 0, 'r*', 1-mu, 0, 0,'g*')


%plot the initial trajectory
%plot3(x(:,1), x(:,2), x(:,3), 'k')

%TEST: Plot the stopping point of x_halo
%plot3(x_halo(1,1), x_halo(1,2), x_halo(1,3), 'r*')
%plot3(x_halo(numSteps,1), x_halo(numSteps,2), x_halo(numSteps,3), 'r*')


%axis([(L1-0.01) (L1+0.01) -0.01 0.01 -0.01 0.01])

